#ifndef RSIM_MULTIBODYSYSTEM_H
#define RSIM_MULTIBODYSYSTEM_H

/* -------------------------------------------------------------------------- *
 * File: MultibodySystem.h                                                    *
 * Authors: Kishor Bhalerao                                                   *
 * Email : kishor8dm@gmail.com                                                *
 * Contributors:                                                              *
 * Email:                                                                     *
 * copyright (c) 2010 Authors.                                                *
 *                                                                            *
 * This program is free software: you can redistribute it and/or modify it    *
 * under the terms of the GNU General Public License as published by the Free *
 * Software Foundation, either version 3 of the License, or any later version.*
 *                                                                            *
 * This program is distributed in the hope that it will be useful, but        *
 * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY *
 * or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License   *
 * for more details. (http://www.gnu.org/licenses/)                           *
 * -------------------------------------------------------------------------- */

#include "List.h"
#include "RigidBodyNode.h"
#include "Stage.h"
#include "State.h"
#include "Index.h"

#include <iostream>
#include <cstdlib>
#include <new>

using namespace std;
namespace RSIM{

class State;
class SystemAcceleration;
class RigidBodyNode;

/** 
This class is an isolated articulated topology. Each MultibodySystem has its own 
State vector and 0 based indexing for it's nodes.
<p>
(kdb: The name of this class is misleading. I would like to think that a MultibodySystem 
includes all the interacting bodies of interest. But this class just includes all the bodies
in a given articulated tree. For the moment, it is the class SystemData which includes all the 
interacting bodies.)
<p>
*/
class MultibodySystem{
        private:
                
                int SysId_;
                
                /** 
                This is the root and the first body which is defined with 
                respect to the ground frame 
                */
                RigidBodyNode *Root_;

                /** 
                Number of nodes in the system. This does not include the
                Ground body. That is for a standard 4 bar mechanism, 
                nNodes_ = 3.
                */
                int nNodes_;		

                /** 
                This includes the pointers to all the bodies in the system 
                in the order they are originally added in the system
                */
                List <RigidBodyNode *> BodyArray_;

                /** True if the function setTopologyMaps is called, false otherwise*/
                bool isTopologyMapSet_;

                /** System Acceleration */
                //TODO This should be shifted to class SystemData
                SystemAcceleration *SysAccl_;				

        protected:
                
                void initJoints();
                
                void initConnectivityMap();
                
                void recursiveSweepForwardRNEA(const int& ChildID, State& state, RigidBodyNode *Nd);
                
                /** 
                Computes all body fixed useful vectors and allocates required space for the 
                transform FM. 
                */
                void initTransformFM();		
                
                void recursiveUpdSystemRotationMatrixNF(const State& state, RigidBodyNode *Nd);
                
                void recursiveUpdSystemNBetak(const State& state, RigidBodyNode *Nd);
                
                void recursiveVelocityUpdate(const State& state, RigidBodyNode *Nd);
                
                void recursiveDeleteNode(RigidBodyNode *Nd);
                
                void recursiveInitEOM(RigidBodyNode *Nd);

                void recursiveInitForwardDynamics(RigidBodyNode *Nd);

                void recursiveSweepBackwardsABA(const int& ChildId,RigidBodyNode *Nd,State& state);

                void recursiveSweepForwardABA(const int& ChildID,RigidBodyNode *Nd, State& state);

                void recursiveSweepBackwardRNEA(const int& ChildId,RigidBodyNode *Nd, State& state);

                /** 
                This function updates the joint transforms for the entire system form the state vector.
                The vector betak is also updated for each body if necessary. 
                */
                void updJointTransforms(const State& state);

        public:
                
                /** Connectivity Map. Gives the path from each body to root node */
                List <List <int> > cMap_;
                                
                /** Create an empty Multibody system. set stage to empty */
                MultibodySystem();
                
                /** Partially implemented */
                ~MultibodySystem();
                
                /** To be implemented */
                MultibodySystem(const MultibodySystem&); 
                
                /** This is the first fuction which must be called
                before adding any other nodes to the tree */
                RigidBodyNode* setRoot(const RigidBody& Data);				
                
                /** Return a pointer to the root of the system, that is the 
                first body in the tree excluding ground. */
                const RigidBodyNode* getRoot() const;
                
                /** Returns body specified by BodyIndex. Ground body is of index 0 and cannot
                be accessed using this function. */
                const RigidBodyNode* getBody(const BodyIndex& BI) const;
                
                /** Returns body specified by BodyIndex. Note, the 0th node is the root of this system and not 
                the ground body. */
                const RigidBodyNode* getNode(const int& NodeIndex)const;
                
                /** Returns body specified by BodyIndex. Note, the 0th node is the root of this system and not 
                the ground body. Same as getNode()*/
                const RigidBodyNode* getRigidBodyNode(const int& NodeIndex)const;
                
                RigidBodyNode* wgetNode(const int& NodeIndex);
                
                RigidBodyNode* wgetBody(const BodyIndex& BI);
                
                /** Add prescribed motion on joint connecting body given by Body Index to its
                parent. */
                void addPrescribedMotion(const BodyIndex& BI, PrescribedMotion* Ptr);
                
                void stopPrescribedMotion(const BodyIndex& BI);
                
                void continuePrescribedMotion(const BodyIndex& BI);
                
                /** Add gravity like acceleration*/
                void addSystemAcceleration(const Vect3& GravityLikeAcceleration);
                
                void addExternalSpatialForce(const BodyIndex& BI, ExternalSpatialForce *F);
                                
                /** Add a new node to the existing node ParentNode. */
                RigidBodyNode* addNodeTo(RigidBodyNode *ParentNode, const RigidBody& Data);
                
                /** Calculate initial Toplogy Maps. This function must absolutely be called before doing
                anything else with the objects of this system. */
                void setTopologyMaps();

                /** 
                This updates the q, and u of all joints with prescribed motion in the system. 
                udots and qdots are not populated with this function call.
                */
                void updStateFromPrescribedMotion(State& state);
                
                /** Allocate space for Newton-Euler equations of motion */
                void initEOM();
                
                /** 
                Allocate space for intermediate quantities required in the 
                Articulated Body Algorithm. 
                */
                void initABA();
                
                /** Backward sweep of Articulated Body Algorithm */
                void sweepBackwardsABA(State& state);
                
                /** Forward sweep of Articulated Body Algorithm */
                void sweepForwardABA(State& state);
                                
                /** Backward Sweep for Recursive Newton Euler*/
                void sweepBackwardRNEA(State& state);
                
                /** Motion must be prescribed on each joint for this to work. This function
                calculates the Newton-Euler equations of motion for each body and also ensures
                that motion is prescribed on each joint. Otherwise, the code will exit with an error
                message */
                void initInverseDynamics();
                
                /** 
                Based on the given state, the joint transformation matrices and the rotation matrix 
                between the F frame of each body and ground is calculated. This is an O(n) operation 
                and requires one sweep of the entire system. Calling this function also updates the 
                nbetak vector if applicable.
                */
                void updSystemPosition(const State& state);
                
                /** 
                Updates angular and linear velocity of the entire system.  Note that the QDot cache 
                is not updated with this function call. QDot gets updated during forward kinematic 
                sweep.
                */
                void updSystemVelocity(const State& state);
                                
                void updSystemAcceleration(State& state);
                
                void calcSystemAcceleration(State& state);	
                
                /** Update Q's of joint attached to body given by BodyIndex. If there is
                a Prescribed motion added to this body, then the *q given as an input is ingored
                and the user defined function is called. */
                void updQ(const BodyIndex& BI, 
                          const double *q, 
                          State& state)const;
                          
                void updQ(const NodeIndex& NI, 
                          const double *q, 
                          State& state)const;
                
                void updOneQ(const BodyIndex& bindex, 
                             const QIndex& qindex, 
                             const double& q, State& state)const;
                
                void updOneU(const BodyIndex& bindex, 
                             const UIndex& uindex, 
                             const double& u, State& state)const;
                
                /** Update U's of joint attached to body given by BodyIndex */
                void updU(const BodyIndex& BI, const double *u, State& state)const;
                
                /** This returns the number of nodes in the tree, NNodes = NBodies - 1 */
                int getNNodes() const;
                                
                /** These are the number of bodies in the system including the ground body */
                int getNBodies() const;
                
                void getConstraintForceActingOnBody(const BodyIndex& BI, double *Fc)const;		
                
                const int& getSysId()const{return SysId_;}
                int& wgetSysId(){return SysId_;}
};

////////////////////////////////////////////////////////////////////////

inline const RigidBodyNode* MultibodySystem::getNode(const int& NodeIndex) const {
        #ifdef RSIM_SAFE
        if (NodeIndex>=nNodes_){
                cout<<"\nRigidBodyNode* getNode(const int&), NodeIndex too high. Highest value can be "
                <<nNodes_-1<<",\tEntered value is "<<NodeIndex<<",\tExiting..\n";
                exit(0);
        }
        #endif
        return BodyArray_[NodeIndex];
}

////////////////////////////////////////////////////////////////////////

inline RigidBodyNode* MultibodySystem::wgetNode(const int& NodeIndex){
        #ifdef RSIM_SAFE
        if (NodeIndex>=nNodes_){
                cout<<"\nRigidBodyNode* getNode(const int&), NodeIndex too high. Highest value can be "
                <<nNodes_-1<<",\tEntered value is "<<NodeIndex<<",\tExiting..\n";
                exit(0);
        }
        #endif
        return BodyArray_[NodeIndex];
}

////////////////////////////////////////////////////////////////////////

inline const RigidBodyNode* MultibodySystem::getRigidBodyNode(const int& NodeIndex) const {
        #ifdef RSIM_SAFE
        if (NodeIndex>=nNodes_){
                cout<<"\nRigidBodyNode* getNode(const int&), NodeIndex too high. Highest value can be "
                <<nNodes_-1<<",\tEntered value is "<<NodeIndex<<",\tExiting..\n";
                exit(0);
        }
        #endif
        return BodyArray_[NodeIndex];
}

////////////////////////////////////////////////////////////////////////

inline const RigidBodyNode* MultibodySystem::getBody(const BodyIndex& BI) const {
        #ifdef RSIM_SAFE
        if (BI.id()>=nNodes_){
                cout<<"\nRigidBodyNode* getBody(const BodyIndex&), BodyIndex too high. Exiting..\n";
                exit(0);
        }
        #endif
        return BodyArray_[BI.id()];
}

////////////////////////////////////////////////////////////////////////

inline RigidBodyNode* MultibodySystem::wgetBody(const BodyIndex& BI){
        #ifdef RSIM_SAFE
        if (BI.id()>=nNodes_){
                cout<<"\nRigidBodyNode* getBody(const BodyIndex&), BodyIndex too high. Exiting..\n";
                exit(0);
        }
        #endif
        return BodyArray_[BI.id()];
}

////////////////////////////////////////////////////////////////////////

} // namespace RSIM
#endif